// -*- C++ -*-
/*!
 * @file  ForceSensor.cpp
 * @brief FS component
 * $Date$
 *
 * $Id$ ForceSensor2
 */

#include <unistd.h>
#include <sys/io.h>
#include "ForceSensor.h"

#define BASEADDR 0x314
#define G 9.8

// Module specification
// <rtc-template block="module_spec">
static const char* forcesensor_spec[] =
{
    "implementation_id", "ForceSensor",
    "type_name",         "ForceSensor",
    "description",       "FS component",
    "version",           "1.0",
    "vendor",            "MS_LAB",
    "category",          "",
    "activity_type",     "",
    "max_instance",      "",
    "language",          "C++",
    "lang_type",         "compile",
    // Configuration variables
    "conf.default.fullscale_Fx[N]", "200",
    "conf.default.fullscale_Fy[N]", "200",
    "conf.default.fullscale_Fz[N]", "400",
    "conf.default.fullscale_Mx[Nm]", "20",
    "conf.default.fullscale_My[Nm]", "20",
    "conf.default.fullscale_Mz[Nm]", "20",
    "conf.default.filter_No.", "1",
    "conf.default.1_toolcoordinate_Tx[mm]", "0",
    "conf.default.2_toolcoordinate_Ty[mm]", "0",
    "conf.default.3_toolcoordinate_Tz[mm]", "0",
    "conf.default.4_toolcoordinate_Rz[deg]", "0",
    "conf.default.5_toolcoordinate_Ry[deg]", "0",
    "conf.default.6_toolcoordinate_Rx[deg]", "0",

    "conf.Grad.fullscale_Fx[N]", "200",
    "conf.Grad.fullscale_Fy[N]", "200",
    "conf.Grad.fullscale_Fz[N]", "400",
    "conf.Grad.fullscale_Mx[Nm]", "20",
    "conf.Grad.fullscale_My[Nm]", "20",
    "conf.Grad.fullscale_Mz[Nm]", "20",
    "conf.Grad.filter_No.", "1",
    "conf.Grad.1_toolcoordinate_Tx[mm]", "0",
    "conf.Grad.2_toolcoordinate_Ty[mm]", "0",
    "conf.Grad.3_toolcoordinate_Tz[mm]", "100",
    "conf.Grad.4_toolcoordinate_Rz[deg]", "0",
    "conf.Grad.5_toolcoordinate_Ry[deg]", "90",
    "conf.Grad.6_toolcoordinate_Rx[deg]", "0",

    ""
};
// </rtc-template>

ForceSensor::ForceSensor(RTC::Manager* manager)
    : RTC::DataFlowComponentBase(manager),
    // <rtc-template block="initializer">
    m_dataoutOut("dataout", m_dataout),
    m_PrvPortPort("PrvPort"),
    // </rtc-template>
    dummy(0)
{
    // Registration: InPort/OutPort/Service
    // <rtc-template block="registration">
    // Set InPort buffers

    // Set OutPort buffer
    registerOutPort("dataout", m_dataoutOut);

    // Set service provider to Ports
    m_PrvPortPort.registerProvider("fs", "FS", m_fs);

    // Set service consumers to Ports

    // Set CORBA Service Ports
    registerPort(m_PrvPortPort);

    // </rtc-template>

}

ForceSensor::~ForceSensor()
{
}


int fs[6]; //フルスケール
int filter=0;  //フィルタ
double tool[6]; //座標変換

RTC::ReturnCode_t ForceSensor::onInitialize()
{
    // <rtc-template block="bind_config">
    // Bind variables and configuration variable
    bindParameter("fullscale_Fx[N]", m_fullscale_Fx, "200");
    bindParameter("fullscale_Fy[N]", m_fullscale_Fy, "200");
    bindParameter("fullscale_Fz[N]", m_fullscale_Fz, "400");
    bindParameter("fullscale_Mx[Nm]", m_fullscale_Mx, "20");
    bindParameter("fullscale_My[Nm]", m_fullscale_My, "20");
    bindParameter("fullscale_Mz[Nm]", m_fullscale_Mz, "20");
    bindParameter("filter_No.", m_filter_No, "1");
    bindParameter("1_toolcoordinate_Tx[mm]", m_toolcoordinate_Tx, "0");
    bindParameter("2_toolcoordinate_Ty[mm]", m_toolcoordinate_Ty, "0");
    bindParameter("3_toolcoordinate_Tz[mm]", m_toolcoordinate_Tz, "0");
    bindParameter("4_toolcoordinate_Rz[deg]", m_toolcoordinate_Rz, "0");
    bindParameter("5_toolcoordinate_Ry[deg]", m_toolcoordinate_Ry, "0");
    bindParameter("6_toolcoordinate_Rx[deg]", m_toolcoordinate_Rx, "0");

    if(iopl(3)<0) {
        std::cout << "Please Login SUPER USER!" << std::endl;
        raise(SIGINT);
    }
    iopl(0);

    // </rtc-template>
    std::cout << "ForceSensor start." << std::endl;
    return RTC::RTC_OK;
}




RTC::ReturnCode_t ForceSensor::onFinalize()
{
    std::cout << "ForceSensor end." << std::endl;
    RTC::Manager::instance().terminate();

    return RTC::RTC_OK;
}



RTC::ReturnCode_t ForceSensor::onActivated(RTC::UniqueId ec_id)
{
    int Force[6], Offset[6];
    int flag = 0;

    ioperm(BASEADDR,4,1);

    //void Set_ToolCoordinate(){
    std::cout << "\nSet_ToolCoordinate." << std::endl;
    double Tool[6];

    Tool[0] = m_toolcoordinate_Tx;
    Tool[1] = m_toolcoordinate_Ty;
    Tool[2] = m_toolcoordinate_Tz;
    Tool[3] = m_toolcoordinate_Rz;
    Tool[4] = m_toolcoordinate_Ry;
    Tool[5] = m_toolcoordinate_Rx;

    for(int i=0; i<3; i++) { //まずエラー判断
        if(fabs(Tool[i]) > 3276) {
            std::cout << "Set_ToolCoordinate ERROR" << std::endl;
            return RTC::RTC_ERROR;
        }
    }
    for(int i=3; i<6; i++) {
        if(fabs(Tool[i]) > 180) {
            std::cout << "Set_ToolCoordinate ERROR" << std::endl;
            return RTC::RTC_ERROR;
        }
    }

    for(int i=0; i<6; i++) { //Toolをtoolに代入＆変換
        tool[i] = Tool[i];
        //std::cout << tool[i] << std::endl;
    }

    outw(0x0200, BASEADDR); //その後座標変換
    outw(1, BASEADDR+2);
    outw(0x0201, BASEADDR);
    outw((int)(tool[0]*10), BASEADDR+2);

    outw(0x0202, BASEADDR);
    outw(2, BASEADDR+2);
    outw(0x0203, BASEADDR);
    outw((int)(tool[1]*10), BASEADDR+2);

    outw(0x0204, BASEADDR);
    outw(3, BASEADDR+2);
    outw(0x0205, BASEADDR);
    outw((int)(tool[2]*10), BASEADDR+2);

    outw(0x0206, BASEADDR);
    outw(6, BASEADDR+2);
    outw(0x0207, BASEADDR);
    outw((int)(-tool[3]*32768/180), BASEADDR+2);

    outw(0x0208, BASEADDR);
    outw(5, BASEADDR+2);
    outw(0x0209, BASEADDR);
    outw((int)(-tool[4]*32768/180), BASEADDR+2);

    outw(0x020a, BASEADDR);
    outw(4, BASEADDR+2);
    outw(0x020b, BASEADDR);
    outw((int)(-tool[5]*32768/180), BASEADDR+2);

    outw(0x020c, BASEADDR);
    outw(0, BASEADDR+2);
    outw(0xe7, BASEADDR);
    outw(0x0500, BASEADDR+2);

    while(flag!=1) {
        outw(0xe7, BASEADDR);
        if(inw(BASEADDR+2) == 0)
            flag=1;
    }

    sleep(1);
    //}

    //void Set_Fullscale(){
    //std::cout << "\nfullscale_Fx:" << m_fullscale_Fx << std::endl;
    //std::cout << "fullscale_Fy:" << m_fullscale_Fy << std::endl;
    //std::cout << "fullscale_Fz:" << m_fullscale_Fz << std::endl;
    //std::cout << "fullscale_Mx:" << m_fullscale_Mx << std::endl;
    //std::cout << "fullscale_My:" << m_fullscale_My << std::endl;
    //std::cout << "fullscale_Mz:" << m_fullscale_Mz << std::endl;
    //std::cout << "filter_No.  :" << m_filter_No    << std::endl;


    if(m_fullscale_Fx < 2 || m_fullscale_Fy < 2 || m_fullscale_Fz < 3
       ||m_fullscale_Mx < 1 || m_fullscale_My < 1 || m_fullscale_Mz < 1
       ||m_filter_No    > 6 || m_filter_No    < 0) {
        std::cout << "\nConfiguration_Parameter ERROR" << std::endl;
        return RTC::RTC_ERROR;
    }

    if(m_fullscale_Fx > 1040 || m_fullscale_Fx < 200 //実際は270。デフォルトに合わせてある
       ||m_fullscale_Fy > 1020 || m_fullscale_Fy < 200 //実際は260
       ||m_fullscale_Fz > 2030 || m_fullscale_Fz < 400 //実際は510
       ||m_fullscale_Mx >   35 || m_fullscale_Mx <   9
       ||m_fullscale_My >   32 || m_fullscale_My <   9
       ||m_fullscale_Mz >   37 || m_fullscale_Mz <  10) {
        std::cout << "\nConfiguration_Parameter Over" << std::endl;
    }

    fs[0] = (int)(m_fullscale_Fx * 10 / G / 2); //フルスケールを本来の定義まで戻す
    fs[1] = (int)(m_fullscale_Fy * 10 / G / 2); //[N] ->[kgf*10]
    fs[2] = (int)(m_fullscale_Fz * 10 / G / 2); //[Nm]->[kgfcm]
    fs[3] = (int)(m_fullscale_Mx *100 / G / 2);
    fs[4] = (int)(m_fullscale_My *100 / G / 2);
    fs[5] = (int)(m_fullscale_Mz *100 / G / 2);
    filter = m_filter_No * 8;                   //フィルタ設定

    std::cout << "Set_Fullscale." << std::endl;

    for(int i=0; i<6; i++) {
        outw(0x80+i, BASEADDR);
        outw(fs[i], BASEADDR+2);
    }

    outw(0xe7, BASEADDR);
    outw(0x0a00, BASEADDR+2);

    sleep(1);
    //}

    //void Reset_Peak(){
    std::cout << "Reset_Peak." << std::endl;
    outw(0x7f, BASEADDR);
    outw(0x90+filter, BASEADDR+2); //フィルタを見る
    outw(0xe7, BASEADDR);
    outw(0x0b00, BASEADDR+2);

    sleep(1);
    //}

    //void Set_Offset(){
    std::cout << "Set_Offset." << std::endl;
    for(int i=0; i<6; i++) {
        outw(0x90+filter+i, BASEADDR); //filterみてる
        Force[i] = inw(BASEADDR+2);
    }

    for(int i=0; i<6; i++) {
        outw(0x88+i, BASEADDR);
        Offset[i] = inw(BASEADDR+2);
    }

    for(int i=0; i<6; i++) {
        Offset[i] += Force[i];
        outw(0x88+i, BASEADDR);
        outw(Offset[i], BASEADDR+2);
    }

    outw(0xe7, BASEADDR);
    outw(0x0700, BASEADDR+2);

    while(flag!=1) {
        outw(0xe7, BASEADDR);
        if(inw(BASEADDR+2) == 0)
            flag=1;
    }

    sleep(1);
    //}

    return RTC::RTC_OK;
}



RTC::ReturnCode_t ForceSensor::onDeactivated(RTC::UniqueId ec_id)
{
    int flag=0;

    //std::cout << "\nForceSensor onDeactivated." << std::endl;

    //Set_ToolCoordinate(FS::DoubleArray2 Tool){
    std::cout << "\nReset_ToolCoordinate." << std::endl;

    ioperm(BASEADDR,4,1);

    for(int i=0; i<6; i++) //tool初期化
        tool[i] = 0;

    outw(0x0200, BASEADDR);
    outw(0, BASEADDR+2);
    outw(0xe7, BASEADDR);
    outw(0x0500, BASEADDR+2);

    while(flag!=1) {
        outw(0xe7, BASEADDR);
        if(inw(BASEADDR+2) == 0)
            flag=1;
    }

    return RTC::RTC_OK;
}



RTC::ReturnCode_t ForceSensor::onExecute(RTC::UniqueId ec_id)
{
    //void Get_Newton(double Force[6]){
    double Force[6];

    ioperm(BASEADDR,4,1);

    for(int i=0; i<6; i++) {
        outw(0x90+filter+i,BASEADDR); //フィルタを見る
        Force[i] = inw(BASEADDR+2);

        if(Force[i]>32767)
            Force[i] = Force[i] - 65536;
    }

    Force[0] = Force[0] * 0.000061035 * fs[0]/10  * G; //[N]
    Force[1] = Force[1] * 0.000061035 * fs[1]/10  * G; //[N]
    Force[2] = Force[2] * 0.000061035 * fs[2]/10  * G; //[N]
    Force[3] = Force[3] * 0.000061035 * fs[3]/100 * G; //[Nm]
    Force[4] = Force[4] * 0.000061035 * fs[4]/100 * G; //[Nm]
    Force[5] = Force[5] * 0.000061035 * fs[5]/100 * G; //[Nm]

    m_dataout.data.length(6);
    for(int i=0; i<6; i++)
        m_dataout.data[i] = Force[i];
    m_dataoutOut.write();

    return RTC::RTC_OK;
}





extern "C"
{

void ForceSensorInit(RTC::Manager* manager)
{
    RTC::Properties profile(forcesensor_spec);
    manager->registerFactory(profile,
                             RTC::Create<ForceSensor>,
                             RTC::Delete<ForceSensor>);
}

};


